﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Research.Kinect.Nui;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;
using System.Windows;
using System.Windows.Controls;
using System.Collections;
using Vector = Microsoft.Research.Kinect.Nui.Vector;

namespace Rowing
{
    class KinectNui
    {
        private Runtime main;
        private Camera camera;
        private MainWindow window;
        private Dictionary<JointID, Brush> jointColors = new Dictionary<JointID, Brush>() { 
            {JointID.HipCenter, new SolidColorBrush(Color.FromRgb(169, 176, 155))},
            {JointID.Spine, new SolidColorBrush(Color.FromRgb(169, 176, 155))},
            {JointID.ShoulderCenter, new SolidColorBrush(Color.FromRgb(168, 230, 29))},
            {JointID.Head, new SolidColorBrush(Color.FromRgb(200, 0,   0))},
            {JointID.ShoulderLeft, new SolidColorBrush(Color.FromRgb(79,  84,  33))},
            {JointID.ElbowLeft, new SolidColorBrush(Color.FromRgb(84,  33,  42))},
            {JointID.WristLeft, new SolidColorBrush(Color.FromRgb(255, 126, 0))},
            {JointID.HandLeft, new SolidColorBrush(Color.FromRgb(215,  86, 0))},
            {JointID.ShoulderRight, new SolidColorBrush(Color.FromRgb(33,  79,  84))},
            {JointID.ElbowRight, new SolidColorBrush(Color.FromRgb(33,  33,  84))},
            {JointID.WristRight, new SolidColorBrush(Color.FromRgb(77,  109, 243))},
            {JointID.HandRight, new SolidColorBrush(Color.FromRgb(37,   69, 243))},
            {JointID.HipLeft, new SolidColorBrush(Color.FromRgb(77,  109, 243))},
            {JointID.KneeLeft, new SolidColorBrush(Color.FromRgb(69,  33,  84))},
            {JointID.AnkleLeft, new SolidColorBrush(Color.FromRgb(229, 170, 122))},
            {JointID.FootLeft, new SolidColorBrush(Color.FromRgb(255, 126, 0))},
            {JointID.HipRight, new SolidColorBrush(Color.FromRgb(181, 165, 213))},
            {JointID.KneeRight, new SolidColorBrush(Color.FromRgb(71, 222,  76))},
            {JointID.AnkleRight, new SolidColorBrush(Color.FromRgb(245, 228, 156))},
            {JointID.FootRight, new SolidColorBrush(Color.FromRgb(77,  109, 243))}
        };
        private ArrayList leftList;
        private ArrayList rightList;
        private ArrayList headList;
        private ArrayList heapList;


        public KinectNui(MainWindow window)
        {
            this.window = window;
        }

        public void Initialize()
        {
            leftList = new ArrayList();
            rightList = new ArrayList();
            headList = new ArrayList();
            heapList = new ArrayList();
            //•	Declares _kinectNui as a Runtime object, which represents the Kinect sensor instance.
            main = new Runtime();

            //Open the video and depth streams, and sets up the event handlers that the runtime calls when a video, depth, or skeleton frame is ready
            //An application must initialize the Kinect sensor by calling Runtime.Initialize before calling any other methods on the Runtime object. 
            main.Initialize(RuntimeOptions.UseColor | RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseColor);

            //To stream color images:
            //  •	The options must include UseColor.
            //  •	Valid image resolutions are Resolution1280x1024 and Resolution640x480.
            //  •	Valid image types are Color, ColorYUV, and ColorYUVRaw.
            main.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.ColorYuv);

            camera = main.NuiCamera;

            main.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(SkeletonFrameReady);
            main.VideoFrameReady += new EventHandler<ImageFrameReadyEventArgs>(NuiVideoFrameReady);
        }

        private void NuiVideoFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            PlanarImage Image = e.ImageFrame.Image;
            window.NuiVideoFrameReady(Image);
            window.processHandler();
        }

        private void SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            //window.imageContainer.Children.RemoveRange(window.initObjectNum, window.imageContainer.Children.Count - window.initObjectNum);
            window.SkeletonFrameReady();
            foreach (SkeletonData data in e.SkeletonFrame.Skeletons)
            {
                //Tracked that defines whether a skeleton is 'tracked' or not. The untracked skeletons only give their position. 
                if (SkeletonTrackingState.Tracked != data.TrackingState) continue;

                Brush brush = new SolidColorBrush(Color.FromRgb(255, 0, 0));
                //window.imageContainer.Children.Add(GetBodySegment(data.Joints, brush, JointID.HipCenter, JointID.Spine, JointID.ShoulderCenter, JointID.Head));

                //Each joint has a Position property that is defined by a Vector4: (x, y, z, w). 
                //The first three attributes define the position in camera space. The last attribute (w)
                //gives the quality level (between 0 and 1) of the 
                foreach (Joint joint in data.Joints)
                {
                    if (joint.Position.W < 0.6f) return;// Quality check 
                    switch (joint.ID)
                    {
                        case JointID.Head:
                            //SetJoint(headList, window.imgHead, joint);
                            SetJoint(headList, null, joint);
                            break;
                        case JointID.HandRight:
                            //SetJoint(rightList, window.imgRightHand, joint);
                            SetJoint(rightList, null, joint);
                            break;
                        case JointID.HandLeft:
                            //SetJoint(rightList, window.imgRightHand, joint);
                            SetJoint(rightList, null, joint);
                            break;
                        case JointID.HipCenter:
                            SetJoint(heapList, null, joint);
                            break;
                    }
                }
            }
        }

        private void SetJoint(ArrayList list, Image image, Joint joint)
        {
            Vector position = joint.Position;
            Line jointLine = new Line();
            var jointPos = GetDisplayPosition(joint);
            jointLine.X1 = jointPos.X - 3;
            jointLine.X2 = jointLine.X1 + 6;
            jointLine.Y1 = jointLine.Y2 = jointPos.Y;
            jointLine.Stroke = jointColors[joint.ID];
            jointLine.StrokeThickness = 6;
            if (image != null)
            {
                Canvas.SetLeft(image, jointPos.X);
                Canvas.SetTop(image, jointPos.Y);
            }
            if (list != null)
                list.Add(joint.Position);
        }

        private Polyline GetBodySegment(JointsCollection joints, Brush brush, params JointID[] ids)
        {
            PointCollection points = new PointCollection(ids.Length);
            for (int i = 0; i < ids.Length; ++i)
            {
                //points.Add(GetDisplayPosition(joints[ids[i]]));
            }

            Polyline polyline = new Polyline();
            polyline.Points = points;
            polyline.Stroke = brush;
            polyline.StrokeThickness = 5;
            return polyline;
        }

        private Point GetDisplayPosition(Joint joint)
        {
            float depthX, depthY;
            main.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
            depthX = Math.Max(0, Math.Min(depthX * 320, 320));  //convert to 320, 240 space
            depthY = Math.Max(0, Math.Min(depthY * 240, 240));  //convert to 320, 240 space
            int colorX, colorY;
            ImageViewArea iv = new ImageViewArea();
            // only ImageResolution.Resolution640x480 is supported at this point
            main.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);

            // map back to skeleton.Width & skeleton.Height
            //return new Point((int)(window.imageContainer.Width * colorX / 640.0) - 30, (int)(window.imageContainer.Height * colorY / 480) - 30);
            return new Point();
        }


        public void CalcSpeed()
        {
            window.boat.SetSpeed(CalcSpeedOne(leftList), CalcSpeedOne(rightList), CalcBalance());
            leftList.Clear();
            rightList.Clear();
            headList.Clear();
            heapList.Clear();
        }

        private float CalcSpeedOne(ArrayList posList)
        {
            Vector speed = new Vector();
            speed.X = 0; speed.Y = 0; speed.Z = 0;
            Vector maxPos = new Vector();
            Vector minPos = new Vector();
            maxPos.X = 0; maxPos.Y = 0; maxPos.Z = 0;
            minPos.X = 0; minPos.Y = 0; minPos.Z = 0;
            for (int i = 0; i < posList.Count; i++)
            {
                Vector pos = (Vector)posList[i];

                if (maxPos.Y < pos.Y)
                    maxPos.Y = pos.Y;
                if (maxPos.Z < pos.Z)
                    maxPos.Z = pos.Z;
                if (minPos.Y > pos.Y)
                    minPos.Y = pos.Y;
                if (minPos.Z > pos.Z)
                    minPos.Z = pos.Z;
                if (i == 0) continue;
                Vector prePos = (Vector)posList[i - 1];
                speed.Y += Math.Abs(pos.Y - prePos.Y);
                speed.Z += Math.Abs(pos.Z - prePos.Z);
            }
            return Math.Abs(speed.Y) * (maxPos.Y - minPos.Y) + Math.Abs(speed.Z) * (maxPos.Z - minPos.Z);
        }

        private float CalcBalance()
        {
            float balance = 0;
            if (headList.Count == 0 || heapList.Count == 0)
                return balance;
            int count = Math.Min(headList.Count, heapList.Count);
            balance = ((Vector)headList[count - 1]).X - ((Vector)heapList[count - 1]).X;
            return balance;
        }

        public void Destroy()
        {
            leftList.Clear();
            rightList.Clear();
            headList.Clear();
            heapList.Clear();
            if (main != null)
                main.Uninitialize();
        }
    }
}
